#include "stdafx.h"
#include "cvxMarker.h"
#include <utility>
#include "cvxLog.h"

using std::vector;


CvxMarker::CvxMarker()
{
	m_id  = -1;
	m_perimeter = -1;
	m_corners.resize(4);
}
CvxMarker::~CvxMarker()
{
	m_corners.clear();
}

void CvxMarker::ComputerPerimeter()
{
	m_perimeter = 0.0;
	for (int i=0;i<4;i++) {
		int j = (i+1)%4;
		float d_x = m_corners[i].x - m_corners[j].x;
		float d_y = m_corners[i].y - m_corners[j].y;
		float d = sqrt(d_x * d_x + d_y * d_y);
		m_perimeter += d;
	}
}
float CvxMarker::GetPerimeter(void)
{
	return m_perimeter;
}

static void warp(const Mat &in,Mat &out, const Size size, const Point2f *points)
{

	//obtain the perspective transform
	Point2f  pointsIn[4], pointsRes[4];
	for (int i=0; i<4; i++)
	{
		pointsIn[i] = points[i];
	}
	pointsRes[0] =  Point2f(0, 0);
	pointsRes[1] =  Point2f(size.width-1, 0);
	pointsRes[2] =  Point2f(size.width-1, size.height-1);
	pointsRes[3] =  Point2f(0, size.height-1);
	Mat M = cv::getPerspectiveTransform(pointsIn, pointsRes);
	cv::warpPerspective(in, out,  M, size);
}

static int hammDistMarker(Mat  bits)
{
	int ids[4][5]=
	{
		{
			1,0,0,0,0
		}
		,
		{
			1,0,1,1,1
		}
		,
		{
			0,1,0,0,1
		}
		,
		{
			0,1,1,1,0
		}
	};
	int dist=0;

	for (int y=0;y<5;y++)
	{
		int minSum = INT_MAX;
		//hamming distance to each possible word
		for (int p=0; p<4; p++)
		{
			int sum=0;
			//now, count
			for (int x=0;x<5;x++)
			{
				sum +=  (bits.at<uchar>(y,x) == ids[p][x])?0:1;
			}
			if (minSum > sum)
			{
				minSum = sum;
			}
		}
		//do the and
		dist+=minSum;
	}
	return dist;
}

static Mat rotate(Mat  in)
{
	Mat out;
	in.copyTo(out);
	for (int i=0; i<in.rows; i++)
	{
		for (int j=0; j<in.cols; j++)
		{
			out.at<uchar>(i,j)=in.at<uchar>(in.cols-j-1,i);
		}
	}
	return out;
}
static int mat2id(Mat &bits)
{
	int val=0;
	for (int y=0;y<5;y++)
	{
		val<<=1;
		if ( bits.at<uchar>(y,1)) val|=1;
		val<<=1;
		if ( bits.at<uchar>(y,3)) val|=1;
	}
	return val;
}
static int getMarkerId(Mat &in, int &nRotations)
{
	assert(in.rows == in.cols);
	assert(in.type() == CV_8UC1);
	assert(in.rows == 70);

	Mat grey;
	grey = in;
	//threshold image
	cv::threshold(grey, grey,125, 255, THRESH_BINARY|THRESH_OTSU);

	//Markers  are divided in 7x7 regions, of which the inner 5x5 belongs to marker info
	//the external border shoould be entirely black
	int swidth = 10;
	for (int y = 0; y < 7; y++)
	{
		int inc = 6;
		//for first and last row, check the whole border
		if (y == 0 || y == 6)
		{
			inc = 1;
		}
		for (int x=0; x<7; x+=inc)
		{
			int Xstart = swidth * x;
			int Ystart = swidth * y;
			Mat square = grey(Rect(Xstart, Ystart ,swidth, swidth));
			int nZ = cv::countNonZero(square);
			if (nZ> (swidth*swidth) /2) {
				return -1;//can not be a marker because the border element is not black!
			}
		}
	}


	//get information(for each inner square, determine if it is  black or white)
	Mat bits = Mat::zeros(5,5,CV_8UC1);	

	for (int y=0;y<5;y++)
	{
		for (int x=0;x<5;x++)
		{
			int Xstart=(x+1)*(swidth);
			int Ystart=(y+1)*(swidth);
			Mat square = grey(Rect(Xstart,Ystart,swidth,swidth));
			int nZ = countNonZero(square);
			if (nZ> (swidth*swidth) /2)
			{
				bits.at<uchar>(y,x) = 1;
			}
		}
	}
	// 		printMat<uchar>( _bits,"or mat");

	//checkl all possible rotations
	Mat Rotations[4];
	int dist;
	Rotations[0] = bits;	
	dist = hammDistMarker(Rotations[0]);

	//first distance, second Id
	std::pair<int,int> minDist(dist, 0);
	for (int i = 1; i<4; i++)
	{
		if (minDist.first == 0)
		{
			break;
		}
		//rotate
		Rotations[i] = rotate(Rotations[i-1]);
		//get the hamming distance to the nearest possible word
		dist = hammDistMarker( Rotations[i]) ;
		if (dist < minDist.first)
		{
			minDist.first =  dist;
			minDist.second = i;
		}
	}

	//not match with marker
	if (minDist.first != 0)
	{
		//@todo: correct if any error
		return -1;
	}
	else 
	{
		nRotations = minDist.second;
		return mat2id(Rotations[ minDist.second]);
	}	
}

bool CvxMarker::tracking(IplImage *grayImage, vector<CvxMarker> &detectedMarkers,
						  int markerNumbers, const MarkerCriteria &criteria, 
						  const vector<CvxMarker> &predictedMarkers)
{
	//check input parameters
	assert(grayImage);
	assert(grayImage->nChannels == 1);	
	assert(markerNumbers == predictedMarkers.size());
	detectedMarkers.clear();
	for (int i = 0; i<predictedMarkers.size(); ++i)
	{
		CvRect r = predictedMarkers[i].m_subArea;
		IplImage *subImage = cvCreateImage(cvSize(r.width, r.height), IPL_DEPTH_8U, 1);
		cvSetImageROI(grayImage, r);
		cvCopy(grayImage, subImage);
		cvResetImageROI(grayImage);

		vector<CvxMarker> subMarker;
		int rst = CvxMarker::detect(subImage, subMarker, 1, criteria);			

		if (rst == CVX_MARKER_SUCCESS)
		{
			subMarker[0].AddOffset(r.x, r.y);
			detectedMarkers.push_back(subMarker[0]);
		}
		else
		{
	//		sprintf(g_log, "tracking marker in sub image failed FILE %s, LINE %d\n", __FILE__, __LINE__);
			cvReleaseImage(&subImage);
			detectedMarkers.clear();
			return false;
		}
		cvReleaseImage(&subImage);
	}
	for (int i = 0; i<detectedMarkers.size(); ++i)
	{
		bool isInside = detectedMarkers[i].GetSubArea(10, grayImage->width, grayImage->height);
		if (!isInside)
		{
			detectedMarkers.clear();
			return false;
		}
	}
	return true;
}

static void cvxFindControus(IplImage *biImage, vector<vector<cv::Point> > &contoursVec, 
							int mode, int method, int minNum, int maxNum)
{
	assert(biImage);
	assert(biImage->nChannels == 1);

	CvMemStorage *mmst = cvCreateMemStorage(0);
	assert(mmst);
	CvSeq * contoure = 0;
	cvFindContours(biImage, mmst, &contoure, sizeof(CvContour), CV_RETR_LIST, method);	
	CvSeq * headContour = contoure;
	while (contoure)
	{
		if (contoure->total >= minNum && contoure->total <= maxNum)
		{
 			vector<cv::Point> cc;
 			cc.resize(contoure->total);
 			for (int i = 0; i<contoure->total; ++i) {
 				CvPoint *p = (CvPoint*)cvGetSeqElem(contoure, i);
 				cc[i] = cvPoint(p->x, p->y);				
 			}
 			contoursVec.push_back(cc);
		}		
		contoure = contoure->h_next;
	}
	if (headContour)
	{
		cvClearSeq(headContour);
	}
	cvReleaseMemStorage(&mmst);
}

void cvxApproxPolyDP(const vector<Point> &curve, vector<Point> & approxCurve,
					 double epsilon, bool closed)
{
	CvMat _ccurve = Mat(curve);
	CvMemStorage *mmst = cvCreateMemStorage(0);

	CvSeq* contoure = cvApproxPoly(&_ccurve, sizeof(CvContour), mmst, CV_POLY_APPROX_DP, epsilon, closed);
	if( contoure && contoure->total > 0 )
	{
		approxCurve.resize(contoure->total);
		for (int i = 0; i<contoure->total; ++i) {
			CvPoint *p = (CvPoint*)cvGetSeqElem(contoure, i);
			approxCurve[i] = cvPoint(p->x, p->y);				
		}		
	}
	if (contoure)
	{
		cvClearSeq(contoure);
	}	
	cvReleaseMemStorage(&mmst);
}


int CvxMarker::detect(IplImage *grayImage, vector<CvxMarker> &detectedMarkers, 
					  int markerNumber, const MarkerCriteria &criteria)
{
	assert(grayImage);
	assert(grayImage->nChannels == 1);

	detectedMarkers.clear();

	vector<vector<cv::Point> > contours;
	vector<vector<cv::Point> > testContouts;

	//stet one: get 4 corner polygon
//	Mat threshImage = cv::Mat(grayImage, true);
	IplImage *threshImage = cvCloneImage(grayImage);
	
	vector<CvxMarker > markerCandidates;
//	cv::adaptiveThreshold(Mat(grayImage), threshImage, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY_INV, 7, 7);
	cvAdaptiveThreshold(grayImage, threshImage, 255, ADAPTIVE_THRESH_MEAN_C, THRESH_BINARY_INV, 7, 7);	
//	cvThreshold(grayImage, threshImage, 85, 255, THRESH_BINARY_INV);
#define MARKER_TEST 0

#if MARKER_TEST

	IplImage *testImage = cvCreateImage(cvSize(grayImage->width/2, grayImage->height/2), IPL_DEPTH_8U, 1);
	cvZero(testImage);	
	cvResize(grayImage, testImage);
	cvShowImage("gray image", testImage);
	cvResize(threshImage, testImage);
	cvShowImage("threshold image", testImage);
#endif
			
//	double tt = clock();
// 	cv::findContours(Mat(threshImage), contours, CV_RETR_TREE, CV_CHAIN_APPROX_NONE);	
	cvxFindControus(threshImage, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE, 
					criteria._minPixels, criteria._maxPixels);
	cvReleaseImage(&threshImage);

#if MARKER_TEST	
	cvx::BwImage t_img(testImage);
	cvZero(testImage);	
	for (int i =0 ;i<contours.size(); ++i)
	{
		for (int j = 0; j<contours[i].size(); ++j)
		{
			int x = contours[i][j].x/2;
			int y = contours[i][j].y/2;
			t_img[y][x] = 255;
		}
	}
	cvShowImage("all contours", testImage);
	cvWaitKey(0);
#endif
	
	for (int i=0; i<contours.size();i++)
	{
		if (contours[i].size() >= criteria._minPixels && contours[i].size() <= criteria._maxPixels)
		{
			vector<Point> approxCurve;			
			float minDist = INT_MAX;
			//cv::approxPolyDP(Mat(contours[i]), approxCurve, 10, true);
			cvxApproxPolyDP(contours[i], approxCurve, 10, true);			
			
			//curve should have 4 corners and is convex
			if (approxCurve.size()!=4 || !cv::isContourConvex(Mat(approxCurve)))
			{
				continue;
			}

			//ensure that the distance between consecutive points is large enough			
			for (int j=0;j<4;j++)
			{
				int d_x = approxCurve[j].x - approxCurve[(j+1)%4].x;
				int d_y = approxCurve[j].y - approxCurve[(j+1)%4].y;
				float d = (float)sqrt( (double)d_x * d_x + d_y * d_y);
				if (d<minDist)
				{
					minDist = d;
				}
			}
			
			//check that distance is not very small
			if (minDist>sqrt((double)criteria._minPixels))
			{
				CvxMarker marker;
				for (int j = 0; j<4; ++j)
				{
					marker.m_corners[j] = Point2f(approxCurve[j]);
				}
				markerCandidates.push_back(marker);
				testContouts.push_back(contours[i]);
			}			
		}
	}

	// not enough markers 
	if(criteria._fastCheck && markerCandidates.size() < markerNumber)
	{
		return CVX_MARKER_LESS_NUMBER;
	}

#if MARKER_TEST
	cvZero(testImage);	
	for (int i =0 ;i<testContouts.size(); ++i)
	{
		for (int j = 0; j<testContouts[i].size(); ++j)
		{
			int x = testContouts[i][j].x/2;
			int y = testContouts[i][j].y/2;
			t_img[y][x] = 255;
		}
	}
	cvShowImage("test contours", testImage);
	cvWaitKey(0);
#endif
	

	//sort the points in anti-clockwise order
	// 0  1
	// 3  2 in image plane
	for (int i=0;i<markerCandidates.size();i++)
	{

		//trace a line between the first and second point.
		//if the third point is at the right side, then the points are anti-clockwise
		double dx1 = markerCandidates[i].m_corners[1].x - markerCandidates[i].m_corners[0].x;
		double dy1 = markerCandidates[i].m_corners[1].y - markerCandidates[i].m_corners[0].y;
		double dx2 = markerCandidates[i].m_corners[2].x - markerCandidates[i].m_corners[0].x;
		double dy2 = markerCandidates[i].m_corners[2].y - markerCandidates[i].m_corners[0].y;
		double dir = (dx1*dy2)-(dy1*dx2);

		if (dir  < 0.0)		 //if the third point is in the left side, then sort in anti-clockwise order
		{
			std::swap(markerCandidates[i].m_corners[1], markerCandidates[i].m_corners[3]);
		}
	}

	//remove these elements whose corners are too close to each other
	//the near pair is caused by adaptive threshold 
	//first detect candidates
	vector<std::pair<int,int>  > TooNearCandidates;
	for (int i=0;i<markerCandidates.size();i++)
	{
		//calculate the average distance of each corner to the nearest corner of the other marker candidate
		for (int j=i+1;j<markerCandidates.size();j++)
		{
			float dist = 0.0;
			for (int k=0; k<4; k++)
			{
				float d_x = markerCandidates[i].m_corners[k].x - markerCandidates[j].m_corners[k].x;
				float d_y = markerCandidates[i].m_corners[k].y - markerCandidates[j].m_corners[k].y;
				dist += sqrt(double(d_x * d_x + d_y * d_y));
			}
			dist /= 4.0;
			//if distance is too small
			if (dist < 10.0) {
				TooNearCandidates.push_back(std::pair<int,int>(i,j));
			}
		}
	}
	
	//computer marker perimeter
	for (int i = 0; i<markerCandidates.size(); ++i)
	{
		markerCandidates[i].ComputerPerimeter();
	}

	//mark for removal the element of  the pair with smaller perimeter
	vector<bool> toRemove (markerCandidates.size());
	for (int i=0; i<toRemove.size(); i++)
	{
		toRemove[i] = false;
	}

	for (unsigned int i=0;i<TooNearCandidates.size();i++)
	{
		if ( markerCandidates[TooNearCandidates[i].first ].m_perimeter > (markerCandidates[TooNearCandidates[i].second].m_perimeter ))
		{
			toRemove[TooNearCandidates[i].second] = true;
		}
		else
		{
			toRemove[TooNearCandidates[i].first] = true;
		}
	}


	///identify the markers
	for (int i=0;i<markerCandidates.size();i++)
	{
		if (!toRemove[i])
		{

			//Find projective homography
			Mat canonicalMarker;
			int nRotations;
			warp(Mat(grayImage), canonicalMarker, Size(70, 70), &(markerCandidates[i].m_corners[0]));			
 			
 			int id = getMarkerId(canonicalMarker,nRotations);
			if (id != -1)
			{
			//	cv::imshow("canonicalMarker", canonicalMarker);
			//	cvWaitKey(0);

				//sort the points so that they are always in the same order as in standard position
				markerCandidates[i].m_id = id;
				std::rotate(markerCandidates[i].m_corners.begin(), 
							markerCandidates[i].m_corners.begin() + 4 - nRotations, 
							markerCandidates[i].m_corners.end());
				detectedMarkers.push_back(markerCandidates[i]);				
			}
		}
	}

	if (criteria._fastCheck && detectedMarkers.size() < markerNumber)
	{
		return CVX_MARKER_LESS_NUMBER;
	}

	
	//small id , larger perimeter put in the front
	std::sort(detectedMarkers.begin(),detectedMarkers.end());

	for (vector<CvxMarker>::iterator it=detectedMarkers.begin(); it != detectedMarkers.end();)
	{
		//remove markers with same Id but small perimeter
		vector<CvxMarker>::iterator next = it + 1;
		while(it != detectedMarkers.end() &&
			  next != detectedMarkers.end() &&
			  next->m_id == it->m_id)
		{
			next = detectedMarkers.erase(next);			
		}
		it = next;
	}

	if (criteria._fastCheck && detectedMarkers.size() < markerNumber)
	{
		return CVX_MARKER_LESS_NUMBER;
	}

	//refine corners
	if (criteria._refineCorner && detectedMarkers.size()>0)
	{
		vector<Point2f> Corners;
		for (int i=0; i<detectedMarkers.size(); i++)
		{
			for (int c=0; c<4; c++)
			{
				Corners.push_back(detectedMarkers[i].m_corners[c]);
			}
		}
		cv::cornerSubPix(Mat(grayImage), Corners, cvSize(5,5), cvSize(-1,-1), 
			         cvTermCriteria (CV_TERMCRIT_ITER|CV_TERMCRIT_EPS,30, 0.01));
		//copy back
		for (unsigned int i=0;i<detectedMarkers.size();i++)
		{
			for (int c=0;c<4;c++)     
			{
				detectedMarkers[i].m_corners[c] = Corners[i*4+c];
			}
		}
	}

	if (detectedMarkers.size() == markerNumber)
	{
		for (int i = 0; i<detectedMarkers.size(); ++i)
		{
			bool isInside = detectedMarkers[i].GetSubArea(10, grayImage->width, grayImage->height);
			if (!isInside)
			{
				detectedMarkers.clear();
				return CVX_MARKER_CLOSE_MARGIN;
			}
			detectedMarkers[i].GetCenterPts();
		//	detectedMarkers[i].ComputerPerimeter();
		}
		return CVX_MARKER_SUCCESS;
	}
	else
	{		
		return CVX_MARKER_DIF_NUMBER;
	}
}

Mat CvxMarker::createMarkerImage(int id, int size) throw (cv::Exception)
{
	assert(id >= 0 && id<1024);
	
	Mat marker(size,size, CV_8UC1);
	marker.setTo(Scalar(0));
	//for each line, create
	int swidth=size/7;
	int ids[4]={0x10,0x17,0x09,0x0e};
	for (int y=0; y<5; y++)
	{
		int index=(id>>2*(4-y)) & 0x0003;
		int val=ids[index];
		for (int x=0; x<5; x++)
		{
			Mat roi=marker(Rect((x+1)* swidth,(y+1)* swidth,swidth,swidth));
			if ( ( val>>(4-x) ) & 0x0001 )
			{
				roi.setTo(Scalar(255));
			}
			else
			{
				roi.setTo(Scalar(0));
			}
		}
	}
	return marker;
}
void CvxMarker::GetCenterPts(void)
{
	m_centerPt = Point2f(0, 0);
	for (int i = 0; i<4; ++i)
	{
		m_centerPt.x += m_corners[i].x;
		m_centerPt.y += m_corners[i].y;
	}
	m_centerPt.x /= 4.0;
	m_centerPt.y /= 4.0;
}

bool CvxMarker::GetSubArea(int dilateLength, int w, int h)
{
	Point2f p1 = Point2f(INT_MAX, INT_MAX);
	Point2f p2 = Point2f(INT_MIN, INT_MIN);
	for (int i = 0; i<4; ++i)
	{
		p1.x = std::min(p1.x, m_corners[i].x);
		p1.y = std::min(p1.y, m_corners[i].y);
		p2.x = std::max(p2.x, m_corners[i].x);
		p2.y = std::max(p2.y, m_corners[i].y);
	}
	CvPoint p3 = cvPoint(cvRound(p1.x), cvRound(p1.y));
	CvPoint p4 = cvPoint(cvRound(p2.x), cvRound(p2.y));

	//too close to image margin
	if (p3.x < 3 || p3.y < 3 || p4.x >= w - 3 || p4.y >=h-3)
	{
		return false;
	}
	p3.x -= dilateLength;
	p3.y -= dilateLength;
	p4.x += dilateLength;
	p4.y += dilateLength;
	
	p3.x = std::max(0, p3.x);
	p3.y = std::max(0, p3.y);
	p4.x = std::min(p4.x, w-1);
	p4.y = std::min(p4.y, h-1);
	m_subArea = cvRect(p3.x, p3.y, p4.x - p3.x, p4.y - p3.y);
	return true;
}

void CvxMarker::AddOffset(int x, int y)
{
	for (int i = 0; i<m_corners.size(); ++i)
	{
		m_corners[i].x +=x;
		m_corners[i].y +=y;
	}
	GetCenterPts();
}


/*

void cv::findContours( InputOutputArray _image, OutputArrayOfArrays _contours,
					  OutputArray _hierarchy, int mode, int method, Point offset )
{
	Mat image = _image.getMat();
	MemStorage storage(cvCreateMemStorage());
	CvMat _cimage = image;
	CvSeq* _ccontours = 0;
	if( _hierarchy.needed() )
		_hierarchy.clear();
	cvFindContours(&_cimage, storage, &_ccontours, sizeof(CvContour), mode, method, offset);
	if( !_ccontours )
	{
		_contours.clear();
		return;
	}
	Seq<CvSeq*> all_contours(cvTreeToNodeSeq( _ccontours, sizeof(CvSeq), storage ));
	int i, total = (int)all_contours.size();
	_contours.create(total, 1, 0, -1, true);
	SeqIterator<CvSeq*> it = all_contours.begin();
	for( i = 0; i < total; i++, ++it )
	{
		CvSeq* c = *it;
		((CvContour*)c)->color = (int)i;
		_contours.create((int)c->total, 1, CV_32SC2, i, true);
		Mat ci = _contours.getMat(i);
		CV_Assert( ci.isContinuous() );
		cvCvtSeqToArray(c, ci.data);
	}

	if( _hierarchy.needed() )
	{
		_hierarchy.create(1, total, CV_32SC4, -1, true);
		Vec4i* hierarchy = _hierarchy.getMat().ptr<Vec4i>();

		it = all_contours.begin();
		for( i = 0; i < total; i++, ++it )
		{
			CvSeq* c = *it;
			int h_next = c->h_next ? ((CvContour*)c->h_next)->color : -1;
			int h_prev = c->h_prev ? ((CvContour*)c->h_prev)->color : -1;
			int v_next = c->v_next ? ((CvContour*)c->v_next)->color : -1;
			int v_prev = c->v_prev ? ((CvContour*)c->v_prev)->color : -1;
			hierarchy[i] = Vec4i(h_next, h_prev, v_next, v_prev);
		}
	}
}
*/

/*
2.3
void cv::findContours( InputOutputArray _image, OutputArrayOfArrays _contours,
                   OutputArray _hierarchy, int mode, int method, Point offset )
{
    Mat image = _image.getMat();
    MemStorage storage(cvCreateMemStorage());
    CvMat _cimage = image;
    CvSeq* _ccontours = 0;
    if( _hierarchy.needed() )
        _hierarchy.clear();
    cvFindContours(&_cimage, storage, &_ccontours, sizeof(CvContour), mode, method, offset);
    if( !_ccontours )
    {
        _contours.clear();
        return;
    }
    Seq<CvSeq*> all_contours(cvTreeToNodeSeq( _ccontours, sizeof(CvSeq), storage ));
    int i, total = (int)all_contours.size();
    _contours.create(total, 1, 0, -1, true);
    SeqIterator<CvSeq*> it = all_contours.begin();
    for( i = 0; i < total; i++, ++it )
    {
        CvSeq* c = *it;
        ((CvContour*)c)->color = (int)i;
        _contours.create((int)c->total, 1, CV_32SC2, i, true);
        Mat ci = _contours.getMat(i);
        CV_Assert( ci.isContinuous() );
        cvCvtSeqToArray(c, ci.data);
    }

    if( _hierarchy.needed() )
    {
        _hierarchy.create(1, total, CV_32SC4, -1, true);
        Vec4i* hierarchy = _hierarchy.getMat().ptr<Vec4i>();
        
        it = all_contours.begin();
        for( i = 0; i < total; i++, ++it )
        {
            CvSeq* c = *it;
            int h_next = c->h_next ? ((CvContour*)c->h_next)->color : -1;
            int h_prev = c->h_prev ? ((CvContour*)c->h_prev)->color : -1;
            int v_next = c->v_next ? ((CvContour*)c->v_next)->color : -1;
            int v_prev = c->v_prev ? ((CvContour*)c->v_prev)->color : -1;
            hierarchy[i] = Vec4i(h_next, h_prev, v_next, v_prev);
        }
    }
}
*/

